#include "astar.h"
#include <queue>
#include <cmath>
#include <limits>
#include <algorithm>
#include <iostream> 

AStar::AStar(Grid& grid) : grid_(grid) {}

double AStar::heuristic(const std::pair<int, int>& a, const std::pair<int, int>& b) {
	return std::abs(a.first - b.first) + std::abs(a.second - b.second);
}


std::vector<std::pair<int, int>> AStar::findPath(const std::pair<int, int>& start, const std::pair<int, int>& goal) {
    using Node = std::pair<std::pair<int, int>, double>; 
    std::priority_queue<Node, std::vector<Node>, std::greater<Node>> open_set; 
    std::map<std::pair<int, int>, double> g_score; 
    std::map<std::pair<int, int>, double> f_score; 
    std::map<std::pair<int, int>, std::pair<int, int>> came_from; 

    open_set.push({start, 0.0});
    g_score[start] = 0.0;
    f_score[start] = heuristic(start, goal);

    while (!open_set.empty()) {
        auto current = open_set.top().first;
        open_set.pop();
	
	if (current == goal) {
	return reconstructPath(current, came_from);
	}


	for (const auto& neighbor : grid_.getNeighbors(current)) {
		double tentative_g_score = g_score[current] + grid_.cost(current, neighbor);

		if (g_score.find(neighbor) == g_score.end() || tentative_g_score < g_score[neighbor]) {
			came_from[neighbor] = current;
			g_score[neighbor] = tentative_g_score;
			f_score[neighbor] = tentative_g_score + heuristic(neighbor, goal);
			open_set.push({neighbor, f_score[neighbor]}); 
		}
	}
   }

    std::cout << "No path found.\n"; 
    return {}; 
}


std::vector<std::pair<int, int>> AStar::reconstructPath(std::pair<int, int> current, std::map<std::pair<int, int>, std::pair<int, int>>& came_from) {
    std::vector<std::pair<int, int>> path = {current};
    while (came_from.find(current) != came_from.end()) {
	current = came_from[current];
	path.push_back(current);
    }

    std::reverse(path.begin(), path.end()); // path反转
    return path;
}
